

#include <std.h>
#include <stdio.h>
#include <string.h>
#include <log.h>
#include <tsk.h>
#include <ecm.h>
#include <gio.h>
#include "ti/pspiom/i2c/I2c.h"
#include "..\libMath\matrix.h"

#include "HMC5883L.h"
#include "err.h"
#include "common.h"
#include "stdio.h"
#include "math.h"
#include "common_i2c.h"
#include "navconbioscfg.h"

//This calibrated coefficients are various in different environment, this is calibrated in SchoolOffice
double mag_13G_cal_invscale[9] = {  1.21262,		0.00269863,	0.0256944,
	       	   	   	   	   	   	   0.00269863,	1.21774,	0.00217981,
	       	   	   	   	   	   	   0.0256944,	0.00217981,	1.34139};
double mag_13G_cal_bias_coe[3] = {-0.5985,	9.9133,	-9.4092};	//3*1, bias_coe * (-1)
/* ========================================================================== */
/*                           FUNCTION DEFINITIONS                             */
/* ========================================================================== */

/* \fn
 *
 * \brief   Function to HMC5883L magnetometer driver.
 *
 */

//read register values start from address addr, length provided by user
int HMC5883L_READ_REG(GIO_Handle inhd,GIO_Handle outhd, Uint8 REG, Uint8 *buf, size_t len)
{
	int error = 0;
	//request to read register @ address *addr
	error = i2cWrite(outhd,HMC5883_ADDR,( unsigned char *)&REG,1,I2c_DEFAULT_WRITE);
	if(error!=IOM_COMPLETED){
		return error;
	}
	//perform read, write to user-defined buffer
    error = i2cRead(inhd,HMC5883_ADDR,( unsigned char *)buf,len,I2c_DEFAULT_READ);
	if(error!=IOM_COMPLETED){
		return error;
	}
	return error;
}

int HMC5883L_WRITE_REG(GIO_Handle inhd,GIO_Handle outhd, Uint8 REG,Uint8 value, op_mode op)	//write a byte to reg, and return if operation is success. 0--success, 1--fail.
{

	int error = 0;
	Uint8 temp[2];

	temp[0] = REG;
	temp[1] = value;
	error = i2cWrite(outhd,HMC5883_ADDR,( unsigned char *)temp,2,I2c_DEFAULT_WRITE);
	if(error!=IOM_COMPLETED){
		return error;
	}
	if(op == VALIDATE){
		error = HMC5883L_READ_REG(inhd,outhd,REG,temp,1);
		if(error){
			//nop
		}else if(temp[0] != value){
			error = IO_VALIDATED_ERR;
		}
	}

 	return error;
}


int HMC5883_SET_DataOutputRate(GIO_Handle inhd,GIO_Handle outhd, Uint8 rate)
{
	Uint8 temp;
	Uint32 error = 0;
	error = HMC5883L_READ_REG(inhd, outhd, HMC5883_R_CONFA, &temp,1);
	if(error){
		return error;
	}
	//configure DO2 to DO0 (Bits 4 - 2)
	temp = (rate << 2) | (temp & 0xE3);
	temp = HMC5883_R_CONFA;
	error = HMC5883L_WRITE_REG(inhd,outhd,HMC5883_R_CONFA,temp,VALIDATE);

	return error;
}


int HMC5883_SET_SampleAvg(GIO_Handle inhd,GIO_Handle outhd, Uint8 num_sample_avg)
{
	Uint8 temp;
	Uint32 error = 0;
	error = HMC5883L_READ_REG(inhd, outhd, HMC5883_R_CONFA, &temp,1);
	if(error){
		return error;
	}
	//configure MA1 to MA0 (Bits 6 - 5)
	temp = (num_sample_avg << 5) | (temp & 0x9F);
	temp = HMC5883_R_CONFA;
	error = HMC5883L_WRITE_REG(inhd,outhd,HMC5883_R_CONFA,temp,VALIDATE);

	return error;
}

int HMC5883_SET_Bias(GIO_Handle inhd,GIO_Handle outhd, Uint8 bias)
{
	Uint8 temp;
	Uint32 error = 0;
	error = HMC5883L_READ_REG(inhd, outhd, HMC5883_R_CONFA, &temp,1);
	if(error){
		return error;
	}
	//configure MS1 to MS0 (Bits 1 - 0)
	temp = (bias) | (temp & 0xFC);
	temp = HMC5883_R_CONFA;
	error = HMC5883L_WRITE_REG(inhd,outhd,HMC5883_R_CONFA,temp,VALIDATE);

	return error;
}


//set operating mode, user defined
int HMC5883_SET_MODE(GIO_Handle inhd,GIO_Handle outhd, Uint8 mode)
{
	return HMC5883L_WRITE_REG( inhd, outhd,  HMC5883_R_MODE, mode,  VALIDATE);
}

int HMC5883_SET_RANGE(GIO_Handle inhd,GIO_Handle outhd, Uint32 range)
{
	Uint8 temp[2];
	Uint32 error = 0;
	error = HMC5883L_READ_REG(inhd, outhd, HMC5883_R_CONFB, temp,1);
	if(error){
		return error;
	}

	temp[1] = (range << 5) | (temp[0] & 0x1F);
	temp[0] = HMC5883_R_CONFB;
	error = HMC5883L_WRITE_REG(inhd,outhd,temp[0],temp[1],VALIDATE);

	return error;
}
/*		if(range == 0)
			mag_scale = 1370;
		else if(range == 1)
			mag_scale = 1090;
		else if(range == 2)
			mag_scale = 820;
		else if(range == 3)
			mag_scale = 660;
		else if(range == 4)
			mag_scale = 440;
		else if(range == 5)
			mag_scale = 390;
		else if(range == 6)
			mag_scale = 330;
		else if(range == 7)
			mag_scale = 230;
*/


int HMC5883_CONNECT(GIO_Handle inhd,GIO_Handle outhd)
{
	Uint8 temp[3];
	int error = 0;
	error = HMC5883L_READ_REG(inhd,outhd,HMC5883_R_IDA,temp,3);
	if(error){
		return error;
	}
	if (temp[0] != 0x48) {
		error = INVAILD_IDA;
    }
    else if (temp[1] != 0x34) {
    	error = INVAILD_IDB;
    }
    else if (temp[2] != 0x33) {
    	error = INVAILD_IDC;
    }
	return error;

}


int HMC5883_GET_RDATA(GIO_Handle inhd,GIO_Handle outhd,short *data){
	Uint8 temp[8];
	int error = 0;

	error = HMC5883L_READ_REG(inhd,outhd,HMC5883_R_XM,temp,6);
	if(error){
		return error;
	}
	data[0] = (((Int16)temp[0]) << 8) | temp[1];	//x
	data[1] = (((Int16)temp[4]) << 8) | temp[5];	//y
	data[2] = (((Int16)temp[2]) << 8) | temp[3];	//z

    return error;

}

//Read magnetic field strength of XYZ from HMC5883
void HMC5883_SCALE_DATA(short *rdata, double *data, int mag_scale)
{
	short x,y,z;

	x= rdata[0];
	y= rdata[1];
	z= rdata[2];
    data[0] = (double)(x) / mag_scale * 0.1 * 1000;	//unit uT
    data[1] = (double)(y) / mag_scale * 0.1 * 1000;
    data[2] = (double)(z) / mag_scale * 0.1 * 1000;
}

void HMC5883_CAL_DATA(short*rdata, double *data, int mag_scale)
//rdata-raw data, data-output 3*1, mag_scale
{
	short x,y,z;
	double temp[3],temp1[3],cal_data[3];

	x= rdata[0];
	y= rdata[1];
	z= rdata[2];
    temp[0] = (double)(x) / mag_scale * 0.1 * 1000;	//unit uT
    temp[1] = (double)(y) / mag_scale * 0.1 * 1000;
    temp[2] = (double)(z) / mag_scale * 0.1 * 1000;
    MatAdd(3, 1, temp, mag_13G_cal_bias_coe, temp1);
    MatMul(3,3,1, mag_13G_cal_invscale, temp1, cal_data);
    data[0] = cal_data[0];
    data[1] = cal_data[1];
    data[2] = cal_data[2];
}

double HMC5883_GETYAW(double pitch, double roll, double *mag_data, double declination)
//input: pitch,roll(rad); mag_data - calibrated mag_data
//output: yaw(rad)
{
	double yaw_temp = 0;
	double Hx,Hy;
	Hx = mag_data[0]*cos(pitch) + mag_data[1]*sin(roll)*sin(pitch) + mag_data[2]*cos(roll)*sin(pitch);
	Hy = mag_data[1]*cos(roll) - mag_data[2]*sin(roll);
	yaw_temp = atan(Hy/Hx);
	if(Hx > 0)
		yaw_temp = -yaw_temp;
	else if(Hy>0 && Hx<0)
		yaw_temp = -PI - yaw_temp;
	else if(Hy<0 && Hx<0)
		yaw_temp = PI - yaw_temp;
	else if(Hx==0 && Hy<0)
		yaw_temp = PI/2;
	else if(Hx==0 && Hy>0)
		yaw_temp = -PI/2;

	yaw_temp = yaw_temp + declination;
	if(yaw_temp > PI)
		yaw_temp = yaw_temp - 2*PI;
	if(yaw_temp < -PI)
			yaw_temp = yaw_temp + 2*PI;
	return yaw_temp;
}

